// -*- coding: utf-8 -*-
#include "mcu_data_sender.h"
#include <locale>
#include <codecvt>

McuDataSender::McuDataSender(ros::NodeHandle& nh) 
    : nh_(nh) {
    // 创建发布器，连接到串口节点的下行数据话题
    downstream_pub_ = nh_.advertise<std_msgs::String>("/serial/downstream", 10);
    
    // 等待连接建立
    ros::Duration(0.5).sleep();
    
    ROS_INFO("McuDataSender 初始化完成，连接到 /serial/downstream");
}

bool McuDataSender::sendJsonKeyValue(const std::string& key, const std::string& value) {
    try {
        nlohmann::json j;
        j[key] = value;
        return sendJsonString(j.dump());
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON字符串失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::sendJsonKeyValue(const std::string& key, int value) {
    try {
        nlohmann::json j;
        j[key] = value;
        return sendJsonString(j.dump());
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON整数失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::sendJsonKeyValue(const std::string& key, double value) {
    try {
        nlohmann::json j;
        j[key] = value;
        return sendJsonString(j.dump());
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON浮点数失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::sendJsonKeyValue(const std::string& key, bool value) {
    try {
        nlohmann::json j;
        j[key] = value;
        return sendJsonString(j.dump());
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON布尔值失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::sendJsonString(const std::string& key, const std::string& value) {
    if (downstream_pub_.getNumSubscribers() == 0) {
        ROS_WARN("没有节点订阅 /serial/downstream 话题");
        return false;
    }
    
    try {
        nlohmann::json j;
        j[key] = value;
        std::string jsonString = j.dump();
        
        std_msgs::String msg;
        msg.data = jsonString;
        downstream_pub_.publish(msg);
        
        ROS_DEBUG("发送JSON: %s", jsonString.c_str());
        return true;
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::sendJsonString(const std::string& jsonString) {
    if (downstream_pub_.getNumSubscribers() == 0) {
        ROS_WARN("没有节点订阅 /serial/downstream 话题");
        return false;
    }
    
    try {
        std_msgs::String msg;
        msg.data = jsonString;
        downstream_pub_.publish(msg);
        
        ROS_DEBUG("发送JSON: %s", jsonString.c_str());
        return true;
    } catch (const std::exception& e) {
        ROS_ERROR("发送JSON失败: %s", e.what());
        return false;
    }
}

bool McuDataSender::hasSubscribers() const {
    return downstream_pub_.getNumSubscribers() > 0;
}

uint32_t McuDataSender::getSubscriberCount() const {
    return downstream_pub_.getNumSubscribers();
}
